# encoding:utf-8
import math
import random
import EventLog as log
import time
import matplotlib.pyplot as plt

class KneeJoint:
    def __init__(self):
        self.zerol = 267.418
        self.minl = 267.418
        self.maxl = 333.500
        self.xlist = []
        self.ylist = []
        self.der = -127.6823551160007  #核心角度+der = 机器人关节角度
        for l in range(267418, 333500, 10):
            l1 = l * 1.0 / 1000
            r = self.calculate_KneeJoint(l1)
            self.xlist.append(l1)
            self.ylist.append(r)

    def __calculate_angles(self,side_a, side_b, side_c):
        # 计算三角形的三个角度（单位为弧度）
        angle_A = math.acos((side_b ** 2 + side_c ** 2 - side_a ** 2) / (2 * side_b * side_c))
        angle_B = math.acos((side_a ** 2 + side_c ** 2 - side_b ** 2) / (2 * side_a * side_c))
        angle_C = math.pi - angle_A - angle_B

        # 将弧度转换为角度
        angle_A = math.degrees(angle_A)
        angle_B = math.degrees(angle_B)
        angle_C = math.degrees(angle_C)

        return angle_A, angle_B, angle_C

    def __calculate_quadrilateral(self,side_a, side_b, side_c, side_d, angle_A):
        # 使用余弦定理计算第三边的长度（注意，需要将angle_A角度转换为弧度）
        side_e = math.sqrt(side_a ** 2 + side_b ** 2 - 2 * side_a * side_b * math.cos(math.radians(angle_A)))
        a1, _, _ = self.__calculate_angles(side_a, side_b, side_e)
        a2, _, _ = self.__calculate_angles(side_c, side_d, side_e)

        return a1 + a2

    def calculate_KneeJoint(self,l):
        # 已知丝杆长度，求膝关节角度
        a, b, c = self.__calculate_angles(257.53, 48, 301.04)
        a1, b1, c1 = self.__calculate_angles(257.53, 90, l)
        # print(a,b,c,a1,b1,c1)
        c2 = c - c1
        rltangle = self.__calculate_quadrilateral(90, 48, 60, 60, c2)
        return rltangle

    def __find_nearest_number(self,lst, target):
        nearest_number = min(lst, key=lambda x: abs(x - target))
        index = lst.index(nearest_number)
        return nearest_number, index

    def angle2lenght(self,in_angle):
        # 给出一个关节角度，得到电机长度
        nearest_number, index = self.__find_nearest_number(self.ylist, in_angle-self.der)
        return self.xlist[index]

    def lenght2angle(self,in_lenght):
        # 给出电机的长度，关节的角度
        nearest_number, index = self.__find_nearest_number(self.xlist, in_lenght)
        return self.ylist[index] + self.der

class ThighJoint:
    def __init__(self):
        self.zerol = 289.689  # 两个丝杆最大值最小值一样
        self.minl = 281.840
        self.maxl = 337.500
        self.xlist = []
        self.ylist = []
        self.der = -99.78001135793549  # 核心角度+der = 机器人关节角度
        for l in range(281840, 337500, 10):
            l1 = l * 1.0 / 1000
            r = self.calculate_KneeJoint(l1)
            self.xlist.append(l1)
            self.ylist.append(r)

    def __calculate_angles(self,side_a, side_b, side_c):
        # 计算三角形的三个角度（单位为弧度）
        angle_A = math.acos((side_b ** 2 + side_c ** 2 - side_a ** 2) / (2 * side_b * side_c))
        angle_B = math.acos((side_a ** 2 + side_c ** 2 - side_b ** 2) / (2 * side_a * side_c))
        angle_C = math.pi - angle_A - angle_B

        # 将弧度转换为角度
        angle_A = math.degrees(angle_A)
        angle_B = math.degrees(angle_B)
        angle_C = math.degrees(angle_C)

        return angle_A, angle_B, angle_C

    def __calculate_quadrilateral(self,side_a, side_b, side_c, side_d, angle_A):
        # 使用余弦定理计算第三边的长度（注意，需要将angle_A角度转换为弧度）
        side_e = math.sqrt(side_a ** 2 + side_b ** 2 - 2 * side_a * side_b * math.cos(math.radians(angle_A)))
        a1, _, _ = self.__calculate_angles(side_a, side_b, side_e)
        a2, _, _ = self.__calculate_angles(side_d, side_c, side_e)

        return a1 + a2

    def calculate_KneeJoint(self,l):
        # 已知丝杆长度，求膝关节角度
        a, b, c = self.__calculate_angles(292.628, 40, 332)
        a1, b1, c1 = self.__calculate_angles(292.628, 80, l)
        c2 = c - c1
        # print(a, b, c, a1, b1, c1,c2)
        rltangle = self.__calculate_quadrilateral(80, 40, 42, 60, c2)
        return rltangle

    def __find_nearest_number(self,lst, target):
        nearest_number = min(lst, key=lambda x: abs(x - target))
        index = lst.index(nearest_number)
        return nearest_number, index

    def angle2lenght(self,in_angle):
        # 给出一个关节角度，得到电机长度
        nearest_number, index = self.__find_nearest_number(self.ylist, in_angle - self.der)
        return self.xlist[index]

    def lenght2angle(self,in_lenght):
        # 给出电机的长度，关节的角度
        nearest_number, index = self.__find_nearest_number(self.xlist, in_lenght)
        return self.ylist[index] + self.der

class AnkleJoint:
    def __init__(self):
        self.zerol = 289.689  # 两个丝杆最大值最小值一样
        self.minl = 255.5
        self.maxl = 333.5
        self.xlist = []
        self.ylist = []
        self.der6 = -81.83973792066688  # 核心角度+der = 机器人关节角度
        for la in range(2445, 3335, 10):
            for lb in range(2445, 3335, 10):
                la1 = la * 1.0 / 10
                lb1 = lb * 1.0 / 10
                r5,r6 = self.calculate_AnkleJoint(la1,lb1)
                self.xlist.append((la1,lb1))
                self.ylist.append((r5,r6))

    def __calculate_angles(self,side_a, side_b, side_c):
        # 计算三角形的三个角度（单位为弧度）
        angle_A = math.acos((side_b ** 2 + side_c ** 2 - side_a ** 2) / (2 * side_b * side_c))
        angle_B = math.acos((side_a ** 2 + side_c ** 2 - side_b ** 2) / (2 * side_a * side_c))
        angle_C = math.pi - angle_A - angle_B

        # 将弧度转换为角度
        angle_A = math.degrees(angle_A)
        angle_B = math.degrees(angle_B)
        angle_C = math.degrees(angle_C)

        return angle_A, angle_B, angle_C

    def __find_nearest_point(self,points, px,py):
        p = [px,py]
        min_distance = math.inf
        nearest_index = None

        for i, point in enumerate(points):
            distance = math.sqrt((point[0] - p[0]) ** 2 + (point[1] - p[1]) ** 2)
            if distance < min_distance:
                min_distance = distance
                nearest_index = i
        return nearest_index

    def calculate_AnkleJoint(self,la, lb):
        lm = 0.5*(la + lb)
        ld = la-lb
        _,_,c = self.__calculate_angles(291.11, 71.021, lm)

        degrees = math.degrees(0.5*ld/(83*0.5))

        # print(lm, c,degrees)

        angle5 = c
        angle6 = degrees

        return angle5,angle6

    def angle2lenght(self,in_angle5,in_angle6):
        # 给出一个关节角度，得到点击长度
        # in_angle6不能超过限度正负18
        t1 = time.time()
        if in_angle6>18:
            log.logger.warning("左右摆动角度不能大于18，实际为%s"%in_angle6)
            in_angle6 = 18
        if in_angle6<-18:
            log.logger.warning("左右摆动角度不能大于18，实际为%s"%in_angle6)
            in_angle6 < -18
        index = self.__find_nearest_point(self.ylist, in_angle5-self.der6,in_angle6)
        return self.xlist[index][0],self.xlist[index][1]

    def lenght2angle(self,in_lenght1,in_lenght2):
        # 给出电机的长度，关节的角度
        # 不能超过限度正负17.3
        if abs(in_lenght1-in_lenght2)>17.3:
            log.logger.warning("两个丝杆长度不能超过17.3，实际为%s" % (in_lenght1-in_lenght2))
        index = self.__find_nearest_point(self.xlist, in_lenght1,in_lenght2)
        return self.ylist[index][0]+self.der6,-self.ylist[index][1]

    def __del__(self):
        pass

def testKneeJoint():
    # 1.膝关节测试
    KJ = KneeJoint()
    # 1.1.单独输入丝杆点击长度，求出核心角度。(注意这里输出的是核心角度，并非关节角度)
    minangle = KJ.calculate_KneeJoint(l=267.418)
    maxangle = KJ.calculate_KneeJoint(l=333.5)
    print("最小角度：", minangle, "丝杆最小长度：", 267.418)
    print("最大角度：", maxangle, "丝杆最大长度：", 333.5)

    # 1.2.随机机10组数据
    for i in range(10):
        lenght = random.uniform(267.418, 333.5)
        angle = KJ.calculate_KneeJoint(l=lenght)
        print("角度：", angle, "丝杆长度：", lenght)
    del KJ

    # 2.所有接口功能
    KJ = KneeJoint()

    # 2.1.获得长度和角度的对应关系，精确到小数点三位
    angleList = KJ.xlist
    lenghtList = KJ.ylist
    # 2.2.通过角度计算长度（根据angleList和lenghtList取最邻近点）
    # 注意，angle2lenght是已经讲核心角度转化成了实际的关节角度
    rlt_lenght = KJ.angle2lenght(226)
    print(rlt_lenght)
    # 2.3.通过长度计算角度（根据angleList和lenghtList取最邻近点）
    rlt_angle = KJ.lenght2angle(332.12)
    print(rlt_angle)

    del KJ

def testThighJoint():
    # 1.大腿关节测试
    TJ = ThighJoint()
    # 1.1.单独输入丝杆点击长度，求出核心角度。(注意这里输出的是核心角度，并非关节角度)
    zeroangle = TJ.calculate_KneeJoint(l=281.84)
    print("最大角度：", zeroangle, "丝杆最小长度：", 281.84)
    minangle = TJ.calculate_KneeJoint(l=260)
    maxangle = TJ.calculate_KneeJoint(l=337.5)
    print("最小角度：", minangle, "丝杆最小长度：", 260)
    print("最大角度：", maxangle, "丝杆最小长度：", 337.5)

    # 1.2.随机机10组数据
    for i in range(10):
        lenght = random.uniform(260, 337.5)
        angle = TJ.calculate_KneeJoint(l=lenght)
        print("角度：", angle, "丝杆长度：", lenght)
    del TJ

    # 2.所有接口功能
    TJ = ThighJoint()

    # 2.1.获得长度和角度的对应关系，精确到小数点三位
    angleList = TJ.xlist
    lenghtList = TJ.ylist
    # 2.2.通过角度计算长度（根据angleList和lenghtList取最邻近点）
    rlt_lenght = TJ.angle2lenght(-100)
    print(rlt_lenght)
    # 2.3.通过长度计算角度（根据angleList和lenghtList取最邻近点）
    rlt_angle = TJ.lenght2angle(281.84)
    print(rlt_angle)

    del TJ

def testAnkleJoint():
    # 1.踝关节测试
    AJ = AnkleJoint()
    # 1.1.输入两个丝杆点击长度，求出踝关节两个核心角度。(注意这里输出的是核心角度，并非关节角度)
    la,lb = 289.689,289.689 # 0点
    angle5,angle6 = AJ.calculate_AnkleJoint(la,lb)
    print(angle5,angle6)

    minangle5, minangle6 = AJ.calculate_AnkleJoint(244.5 , 244.5)
    print("最小最小角度：", minangle5, minangle6, "丝杆长度：", 244.5, 244.5)
    minangle5, maxangle6 = AJ.calculate_AnkleJoint(333.5, 244.5)
    print("最大最小角度：", minangle5, maxangle6, "丝杆长度：", 333.5, 244.5)
    maxangle5, minangle6 = AJ.calculate_AnkleJoint(244.5, 333.5)
    print("最小最大角度：", maxangle5, minangle6, "丝杆长度：", 244.5, 333.5)
    maxangle5, maxangle6 = AJ.calculate_AnkleJoint(333.5, 333.5)
    print("最大最大角度：", maxangle5, maxangle6, "丝杆长度：", 333.5, 333.5)
    mangle5, angle6 = AJ.calculate_AnkleJoint(289.698+30+10 , 289.698+30-10)
    print("踝关节摆动角度：", mangle5, angle6, "丝杆长度：", 289.698, 289.698)

    # 2.2.随机机10组数据
    for i in range(10):
        lenght = (random.uniform(244.5, 333.5),random.uniform(244.5, 333.5))
        angle5,angle6 = AJ.calculate_AnkleJoint(lenght[0],lenght[1])
        print("角度：", angle5,angle6, "丝杆长度：", (lenght[0],lenght[1]))
    del AJ

    # 2.所有接口功能
    AJ = AnkleJoint()
    # 2.1.获得长度和角度的对应关系，精确到小数点三位
    lenghtList = AJ.xlist
    angleList = AJ.ylist

    # 2.2.通过角度计算长度（根据angleList和lenghtList取最邻近点）
    # 可以从上面的随机数中，任意取值验证
    l5,l6 = AJ.angle2lenght(39,20)
    print(l5,l6)
    # 2.3.通过长度计算角度（根据angleList和lenghtList取最邻近点）
    angle5,angle6 = AJ.lenght2angle(333.5,244.5)
    print(angle5,angle6)
    del AJ

    # 4.画图，可视化运动空间
    AJ = AnkleJoint()
    lenghtList = AJ.xlist
    angleList = AJ.ylist
    y1list = []
    y2list = []
    for y in angleList:
        if -61.893<y[0]<141:
            if -18<y[1]<18:
                y1list.append(y[0])
                y2list.append(y[1])
    plt.scatter(y1list, y2list)
    plt.xlabel('angle5')
    plt.ylabel('angle6')
    plt.show()
    del AJ

if __name__ == "__main__":
    KJ = KneeJoint()
    print(KJ.angle2lenght(21))
    # testKneeJoint()
    # testThighJoint()
    testAnkleJoint()
